/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package search;

import java.util.Comparator;
import java.util.HashSet;
import java.util.PriorityQueue;
import java.util.Queue;
import java.util.Set;
import plot.GNUPlot;

/**
 *
 * @author dheath
 */
public class UniformCostSearch implements Search
{
    private int nodesChecked;
    public PriorityNode searchWorld(WorldNode start, WorldNode end, GNUPlot plot)
    {
        nodesChecked = 0;
        Queue<PriorityNode> queue = new PriorityQueue(11, new PriorityNodeComparator());

        Set<WorldNode> visited = new HashSet();
        queue.add(new PriorityNode(start, 0, null,0));
        PriorityNode endPath = null;

        double diagCost = Math.sqrt(start.getLength() * start.getLength() + start.getLength() * start.getLength()) - start.getLength();
        System.out.println(start.getLength()+","+end.getCost());
        //double costSoFar = 0.0;
        while (!queue.isEmpty())
        {
            PriorityNode pcurrent = queue.remove();

            WorldNode current = pcurrent.getNode();

            if (current == end)
            {
                endPath = pcurrent;
                break;
            }

            if (!visited.contains(current))
            {
                nodesChecked++;
                //costSoFar += pcurrent.getTotalCost();
                plot.printNode(current.getCenter(), 1);
                plot.printX(current.getCenter(), 1);

                visited.add(current);


                if (current.getWest() != null && current.getWest().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getWest(), 0, pcurrent,0 + pcurrent.getTotalCost()));
                }
                if (current.getSouthWest() != null && current.getSouthWest().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getSouthWest(), 0, pcurrent,diagCost + pcurrent.getTotalCost()));
                }
                if (current.getSouth() != null && current.getSouth().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getSouth(), 0, pcurrent, 0 + pcurrent.getTotalCost()));
                }
                if (current.getSouthEast() != null && current.getSouthEast().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getSouthEast(), 0, pcurrent,diagCost + pcurrent.getTotalCost()));
                }
                if (current.getEast() != null && current.getEast().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getEast(), 0, pcurrent, 0 + pcurrent.getTotalCost()));
                }
                if (current.getNorthEast() != null && current.getNorthEast().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getNorthEast(), 0, pcurrent,diagCost + pcurrent.getTotalCost()));
                }
                if (current.getNorth() != null && current.getNorth().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getNorth(), 0, pcurrent,0 + pcurrent.getTotalCost()));
                }
                if (current.getNorthWest() != null && current.getNorthWest().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getNorthWest(), 0, pcurrent, diagCost + pcurrent.getTotalCost()));
                }
            }

        }

        printPath(endPath, plot);
        return endPath;
    }

    private void printPath(PriorityNode end, GNUPlot plot)
    {
        double cost = end.getTotalCost();
        int num = 0;
        WorldNode current = null;
        if (end != null)
        {
            current = end.getNode();
            end = end.getParent();
        }
        while (end != null)
        {
            num++;
            //cost += current.getCost();
            plot.printLine(current.getCenter(), end.getNode().getCenter(), 2, true);
            current = end.getNode();

            end = end.getParent();
        }
        System.out.println("TOTAL COST OF PATH: "+cost);
        System.out.println("Total Nodes in path: "+num);
        System.out.println("Total nodes expanded: "+nodesChecked);
    }
    
    class PriorityNodeComparator implements Comparator
    {

        public int compare(Object o1, Object o2)
        {
            PriorityNode pn1 = (PriorityNode) o1;
            PriorityNode pn2 = (PriorityNode) o2;

            return (int) (pn1.getTotalCost() - pn2.getTotalCost());
        }
    }
}
